#include "user_device.h"
#include "user_global.h"
#include "gpio.h"
#include "tim.h"
#include "modbus.h"
#include "math.h"
#include "crc.h"
#include <string.h>
#include "usart.h"
#include "speed_cntr.h"
#include <stdlib.h>
#include <setjmp.h>
#include "wwdg.h"

unsigned char server_address  = 0x10;

bit rack_importl_state  = 0;
bit rack_importr_state  = 0;
bit rack_export_state  = 0;

// 传感器有效标志，只要检测到传感器发生沿变化，认为有效
bit ready_active[STATION_COUNT] = {0};
bit fixed_active = 1;
bit hat_active = 1;
bit tube_active = 1;

bit rack_ready_export = 0;                 // T1动作后，确认rack已在export到位时为1
bit pending_action = 0;                    // 新动作
uchar pending_role = 0;                    // 动作请求类别: 0 key1按键动作请求 1 key2按键动作请求; 2上位机动作
extern jmp_buf main_loop_env;
ulong import_stamp,export_stamp;

#define KEY_STOP_TIMER                       0xFFFFFFFF
#define SHAKE_LASTTIME                       2000            // 按键触发后，去抖确认时长us
#define KEY_PUSHED_CONFIRM_LASTTIME          100000          // 按键触发后，确认持续时长us
#define KEY_PUSHED_LONG_CONFIRM_LASTTIME     8000000         // 按键触发后，长按确认持续时长us
#define KEY_EVENT_COUNT                 3
ulong key_stamp[3] = { KEY_STOP_TIMER },shake_stamp[3] = { KEY_STOP_TIMER};
ulong key_longtime_stamp[3] = { KEY_STOP_TIMER };
uchar event[KEY_EVENT_COUNT];                                         // 左右缓冲输入事件
ushort auto_key_pending[KEY_EVENT_COUNT] = {0};                       // 按键触发事件申请的动作计数

bit led_update_disable = 0;

enum LED_MODE{
    LED_ON_CONST,           // 常亮
    LED_OFF_CONST,          // 常灭
    LED_BLINK_INFO,         // 闪烁
    LED_BLINK_WARNINNG,     // 闪烁
    LED_BLINK_ERR,          // 闪烁
    LED_BLINK_FATAL,        // 闪烁
    LED_BLINK_AUTOMATIC,    // 闪烁

    LED_MODE_COUNT,
};

LED_CNTRL led_mode_pool[LED_MODE_COUNT] = {
    {0xFFFF,    1,      0},
    {0xFFFF,    0,      1},
    {0xFFFF,    1000,   1000},
    {0xFFFF,    500,    500},
    {0xFFFF,    200,    200},
    {0xFFFF,    100,    100},
    {0xFFFF,    2000,   2000},
};

#define MAX_FIX_COUNT     20
FIX_VALUE fix;

//////////////////////////////////////////////////////////////
void Update_IO(void)
{
    ro_bit.s1_at_home  = IO_Read(IO_S1_HOME);
    ro_bit.s2_at_home  = IO_Read(IO_S2_HOME);
    ro_bit.v1_at_home  = IO_Read(IO_V1_HOME);
    ro_bit.v2_at_home  = IO_Read(IO_V2_HOME);
    ro_bit.v3_at_home  = IO_Read(IO_V3_HOME);
    ro_bit.s1_at_limit = IO_Read(IO_S1_LIMIT);
    ro_bit.s2_at_limit = IO_Read(IO_S2_LIMIT);

    ro_bit.basket_exist_prel = IO_Read(IO_BASKETL_EXIST);
    ro_bit.basket_exist_prer = IO_Read(IO_BASKETR_EXIST);

    if(ro_bit.tube_exist != IO_Read(IO_TUBE_EXIST))
    {
        ro_bit.tube_exist = IO_Read(IO_TUBE_EXIST);
        tube_active = 1;
    }
    if(ro_bit.rack_fixed != IO_Read(IO_RACK_FIXED))
    {
        ro_bit.rack_fixed = IO_Read(IO_RACK_FIXED);
        fixed_active = 1;
    }
    if(ro_bit.hat_exist != IO_Read(IO_HAT_EXIST))
    {
        ro_bit.hat_exist = IO_Read(IO_HAT_EXIST);
        hat_active = 1;
    }

    if(ro_bit.rack_ready_importl != IO_Read(IO_IMPORTL_READY))
    {
        ro_bit.rack_ready_importl  = IO_Read(IO_IMPORTL_READY);
        if(!ro_bit.rack_ready_importl)
            ro_word.rack_importl_refer_count = 0;
        ready_active[IMPORTL] = 1;
    }

    if(ro_bit.rack_ready_importr != IO_Read(IO_IMPORTR_READY))
    {
        ro_bit.rack_ready_importr  = IO_Read(IO_IMPORTR_READY);
        if(!ro_bit.rack_ready_importr)
            ro_word.rack_importr_refer_count = 0;
        ready_active[IMPORTR] = 1;
    }

    if(ro_bit.rack_ready_export != IO_Read(IO_EXPORT_READY))
    {
        ro_bit.rack_ready_export = IO_Read(IO_EXPORT_READY);
        if(!ro_bit.rack_ready_export)
        {
            ro_word.rack_export_refer_count = 0;
            rack_ready_export = 0;
        }
        ready_active[EXPORT] = 1;
    }

    if(ro_bit.rack_exist_prel != IO_Read(IO_PREL_EXIST))
    {
        ro_bit.rack_exist_prel = IO_Read(IO_PREL_EXIST);
        if(!IO_Read(IO_S1_HOME) && ro_bit.rack_exist_prel)
            Start_Beep(BEEP_ERR);
    }
    if(ro_bit.rack_exist_prer != IO_Read(IO_PRER_EXIST))
    {
        ro_bit.rack_exist_prer = IO_Read(IO_PRER_EXIST);
        if(!IO_Read(IO_S2_HOME) && ro_bit.rack_exist_prer)
            Start_Beep(BEEP_ERR);
    }
}

///////////////////////////////////////////////////////////////
/// app_args
bit Ignore_Importl_Ready()
{
    return rw_word.app_arg1.ignore_importl_ready;
}

bit Ignore_Importr_Ready()
{
    return rw_word.app_arg1.ignore_importr_ready;
}

bit Ignore_Export_Ready()
{
    return rw_word.app_arg1.ignore_export_ready;
}

bit Ignore_Import_Ready(uchar index)
{
    assert(index < 2);
    if(index == 0)
        return rw_word.app_arg1.ignore_importl_ready;
    if(index == 1)
        return rw_word.app_arg1.ignore_importr_ready;
    return 0;
}

///////////////////////////////////////////////////////////////
// 返回该节点是否存在试管架
bit Rack_Exist(uchar station)
{
    // 如果忽略传感器错误，且未检测到传感器有效，使用refer_count
    // 如果未忽略传感器错误，或检测到传感器有效，使用传感器状态或refer_count
    switch (station) {
    case IMPORTL:
        if(Ignore_Import_Ready(IMPORTL) && !ready_active[IMPORTL])
            return ro_word.rack_importl_refer_count > 0;
        else if(ro_bit.rack_ready_importl || ro_word.rack_importl_refer_count)
            return 1;

        break;
    case IMPORTR:
        if(Ignore_Import_Ready(IMPORTR) && !ready_active[IMPORTR])
            return ro_word.rack_importr_refer_count > 0;
        else if(ro_bit.rack_ready_importr || ro_word.rack_importr_refer_count)
            return 1;

        break;
    case EXPORT:
        if(Ignore_Export_Ready() && !ready_active[EXPORT])
            return ro_word.rack_export_refer_count > 0;
        else if(ro_bit.rack_ready_export || ro_word.rack_export_refer_count)
            return 1;
        break;
    default:
        break;
    }
    return 0;
}

///////////////////////////////////////////////////////////////
// start event
#define MAX_RETRY_TIMES     1
#define MAX_MACHINE_OCUNT   KEY_COUNT

uchar me_state[MAX_MACHINE_OCUNT] = {ME_STATE_IDLE};
uchar me_retry[MAX_MACHINE_OCUNT] = {0};
uchar me_lastEvent[MAX_MACHINE_OCUNT] = {0};        // 最后一次记录的异常事件
bit high_priority_me = 0;
uchar me_max_retry_times[MAX_MACHINE_OCUNT] = {MAX_RETRY_TIMES,MAX_RETRY_TIMES,MAX_RETRY_TIMES};
uchar me_should_wait_ready[MAX_MACHINE_OCUNT] = {0};
uchar import_need_close_gate[2] = {0};
bit modbus_key_trigger[KEY_COUNT] = {0};
bit mkey_changed[KEY_COUNT] = {0};
ulong mkey_stamp[KEY_COUNT] = {TIMER_STOPED, TIMER_STOPED, TIMER_STOPED};

bit Modbus_Key_Trigger(uchar index)
{
    bit v;
    if(modbus_key_trigger[index])
    {
        modbus_key_trigger[index] = 0;
        if(mkey_stamp[index] == TIMER_STOPED ||
            (mkey_stamp[index] != TIMER_STOPED && TIMER_OUT(mkey_stamp[index], 50000)))
        {
            TIMER_START(mkey_stamp[index]);

            v = index ? rw_bit.start_r : rw_bit.start_l;
            if(v == 0)
            {
                if(me_state[index] == ME_STATE_WAIT || me_state[index] == ME_STATE_WORKING
                    || me_state[index] == ME_STATE_RESET)
                {
                    return 1;
                }
                else
                {
                    return 0;
                }
            }
            else
            {
                if(me_state[index] == ME_STATE_IDLE)
                    return 1;
            }

        }
    }
    return 0;
}

// 检测到按键按下返回1,否则返回0
bit Key_Pushed(uchar key)
{

    bit pressed;
    bit key_long_pressed;
    uchar key_index =  key - IO_STARTL_PRESSED ;
    assert(key == IO_STARTL_PRESSED || key == IO_STARTR_PRESSED || key == IO_STOP_PRESSED)

    pressed = IO_Read(key);
    // 检测按键状态变化
    if(pressed != Test_Bit(RO_BIT_FIRST, STARTL_PRESSED + key_index))
    {
         Set_Bit(RO_BIT_FIRST, STARTL_PRESSED + key_index, pressed);
         if(pressed)
         {
             if(key_stamp[key_index] == TIMER_STOPED)
             {
                 TIMER_START(key_stamp[key_index]);
             }
             TIMER_START(shake_stamp[key_index]);
         }
         else {
         }
    }

    if(!pressed)
    {
        if(shake_stamp[key_index] != TIMER_STOPED && SystemTick() - shake_stamp[key_index] > SHAKE_LASTTIME)
        {
            TIMER_STOP(key_stamp[key_index]);
            TIMER_STOP(shake_stamp[key_index]);
            TIMER_STOP(key_longtime_stamp[key_index]);
        }
        return 0;
    }
    else {

        if(key_stamp[key_index] != TIMER_STOPED)
            (&ro_word.startl_pressed_duration)[key_index] = (SystemTick() - key_stamp[key_index]) / 1000;
    }

    if(key_longtime_stamp[key_index] != TIMER_STOPED && SystemTick() - key_longtime_stamp[key_index] > KEY_PUSHED_LONG_CONFIRM_LASTTIME)
    {
        TIMER_STOP(key_longtime_stamp[key_index]);
        if(!key_index)
            rw_bit.key_long_pressed_l = 1;
        else
            rw_bit.key_long_pressed_r = 1;

        key_long_pressed = ro_bit.stop_pressed;
        ro_bit.stop_pressed = key_long_pressed ? 0 : 1;
        if(ro_bit.stop_pressed)
        {
            Start_Beep(BEEP_WARNING);
            Set_Starts_Led(0, LED_BLINK_AUTOMATIC);
            Set_Starts_Led(1, LED_BLINK_AUTOMATIC);
            led_update_disable = 1;
        }
        else
        {
            Start_Beep(BEEP_INFO2);
            led_update_disable = 0;
            Set_Starts_Led(0, LED_OFF_CONST);
            Set_Starts_Led(1, LED_OFF_CONST);
        }
    }

    if(key_stamp[key_index] != TIMER_STOPED && SystemTick() - key_stamp[key_index] > KEY_PUSHED_CONFIRM_LASTTIME)
    {
        TIMER_START(key_longtime_stamp[key_index]);
        TIMER_STOP(key_stamp[key_index]);
        return 1;
    }

    return 0;
}

// 软开关指示灯控制函数,周期 == 亮-->灭 | 亮-->灭 |......
void Key_Led_Ctrl(uchar n)
{
#define LED_BLINK_FORERVER            0xFFFF        // led 始终按设定模式执行

#define STATE_READY_TO_LIGHT_OFF      0             // 准备置灭LED
#define STATE_READY_TO_LIGHT_ON       1             // 准备点亮LED

    static uchar led_state[2] = {0};
    static ulong led_on_time[2] = {0};
    static ulong led_off_time[2] = {0};
    static ushort led_on_duration[2] = {0};

    LED_CNTRL *led = &rw_word.led_cnt[n];
    if(led_on_duration[n]!= led->led_on_duration)
    {
        if(!led_on_duration[n])
        {
            IO_Set(IO_STARTL_LED + n);
            led_state[n] = STATE_READY_TO_LIGHT_OFF;
            led_on_time[n] = SystemTick();
        }
        led_on_duration[n] = led->led_on_duration;
    }

    if(led->led_blink_times)
    {
        switch (led_state[n]) {
        case STATE_READY_TO_LIGHT_OFF:
            if(led->led_off_duration)
            {
                // 亮时间超时，准备置灭
                if((SystemTick() - led_on_time[n]) > (ulong)led->led_on_duration * 1000 )
                {
                    IO_Reset(IO_STARTL_LED + n);
                    led_state[n] = STATE_READY_TO_LIGHT_ON;
                    led_off_time[n] = SystemTick();
                }
            }
            else {
                led_state[n] = STATE_READY_TO_LIGHT_ON;
            }
            break;
        case STATE_READY_TO_LIGHT_ON:
            if(led->led_on_duration)
            {
                // 灭时间超时，准备点亮
                if( (SystemTick() - led_off_time[n]) > (ulong)led->led_off_duration * 1000 )
                {
                    IO_Set(IO_STARTL_LED + n);
                    led_state[n] = STATE_READY_TO_LIGHT_OFF;
                    led_on_time[n] = SystemTick();
                    if(led->led_blink_times != LED_BLINK_FORERVER)
                        --led->led_blink_times;
                }
            }
            else {
                led_state[n] = STATE_READY_TO_LIGHT_OFF;
            }
        default:
            break;
        }
    }
}

void Set_Starts_Led(uchar n, uchar model)
{
    if(led_update_disable)
        return;
    memcpy(&rw_word.led_cnt[n],&led_mode_pool[model],sizeof(LED_CNTRL));
}

bit All_Starters_Idle()
{
    uchar i;
    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        if(me_state[i] != ME_STATE_IDLE)
            return 0;
    }
    return 1;
}

bit Other_Starters_Idle(uchar starter)
{
    uchar i;
    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        if(i == starter)
            continue;

        if(me_state[i] != ME_STATE_IDLE)
            return 0;
    }
    return 1;
}

bit All_Starters_Wait()
{
    uchar i;
    for(i = 0; i < KEY_START_COUNT; ++i)
    {
        if(me_state[i] != ME_STATE_WAIT)
            return 0;
    }
    return 1;
}

void Clear_Event(uchar index)
{
    event[index] = EVENT_NONE;
}

void Post_Event(uchar index, uchar etype)
{
    if(index < KEY_EVENT_COUNT)
        event[index] = etype;
}

uchar Take_Event(uchar index)
{
    uchar e;
    if(index >= KEY_EVENT_COUNT)
        return 0;
    e = event[index];
    event[index] = 0;
    return e;
}

void Me_Init(uchar index)
{
    auto_key_pending[index] = 0;
    Clear_Event(index);
    me_lastEvent[index] = EVENT_NONE;
}

// 按钮开关按键触发处理
void Process_Start_Event(uchar index)
{
    bit key_pushed = Key_Pushed(!index ? IO_STARTL_PRESSED:IO_STARTR_PRESSED);

    if(key_pushed == 0)
        key_pushed = Modbus_Key_Trigger(index);

    if(!key_pushed)
        return;

    mkey_changed[index] = 1;
    Start_Beep(BEEP_USER_KEY_PUSHED);

    switch(me_state[index])
    {
    case ME_STATE_IDLE:
        me_state[index] = ME_STATE_WAIT;
        Set_Starts_Led(index, LED_BLINK_INFO);
        Me_Init(index);
        me_retry[index] = 0;
        me_max_retry_times[index] = MAX_RETRY_TIMES;
        me_should_wait_ready[index] = 0;
        break;
    case ME_STATE_WAIT:
        if(!IO_Read(IO_S1_HOME+index))
        {
            me_state[index] = ME_STATE_RESET;
            Do_Action_Reset_NoWait(index);
            break;
        }
        me_state[index] = ME_STATE_IDLE;
        Set_Starts_Led(index, LED_OFF_CONST);
        break;
    case ME_STATE_WORKING:
        if(!IO_Read(IO_S1_HOME+index))
        {
            me_state[index] = ME_STATE_RESET;
            Do_Action_Reset_NoWait(index);
            break;
        }
        else
        {
            Do_Action_Stop(index);
            me_state[index] = ME_STATE_IDLE;
            Set_Starts_Led(index, LED_OFF_CONST);
        }
        break;
    case ME_STATE_RESET:
        Do_Action_Stop(index);
        me_state[index] = ME_STATE_IDLE;
        Set_Starts_Led(index, LED_OFF_CONST);
        break;
    }
}

void Check_Improper_Input(void)
{
    uchar index = 0;
    for(index = 0; index < 2; ++index)
    {
        if(!IO_Read(IO_S1_HOME+index) && IO_Read(IO_PREL_EXIST+index))
            Start_Beep(BEEP_ERR);
    }
}

bit Can_Lock_Export(bit ignore_error)
{
    if(ps_server_busy)
        return 0;
    if(ro_word.server_status != STATUS_IDLE && !ignore_error)
        return 0;

    if(Rack_Exist(EXPORT)&& rack_ready_export)
        return 0;

    return 1;
}

bit S_Can_Start(uchar index)
{
    if(!IO_Read(IO_BASKETL_EXIST+index))
        return 0;

    if(Test_Bit(RW_BIT_FIRST, 7+index))
        return 0;

    return 1;
}

bit S_Need_Wait_Ready(uchar index)
{
    if(!IO_Read(IO_BASKETL_EXIST+index))
        return 1;
    return 0;
}

bit Maybe_Terminated(uchar index)
{
    switch(index)
    {
    case S1:
    case S2:
        // 如果检测到试管架存在，停止动作
        if(IO_Read(IO_PREL_EXIST + index - S1))
        {
            Do_Action_Stop(index);
            return 1;
        }
        break;
    default:
        return 0;
    }
    return 0;
}

void Process_Rack(uchar index)
{
    uchar event = 0;
    bit ignore_error = 0;
    static ulong me_stamp[KEY_START_COUNT] = {0};
    assert(index < KEY_START_COUNT);

    switch(me_state[index])
    {
    case ME_STATE_IDLE:
        break;
    case ME_STATE_WAIT:
        if(!Other_Starters_Idle(index))
        {
            if(All_Starters_Wait() && high_priority_me == index)
            {
                // 所有starter都在等待，且当前等待优先级最高
            }
            else if(All_Starters_Wait() && high_priority_me != index)
            {
                // 分析是或否高优先级的受到了硬件条件制约
                if (!S_Can_Start(high_priority_me))
                    high_priority_me = index;
            }
            else
                return;
        }
        if(!Can_Lock_Export(1))
            return;

        if(S_Need_Wait_Ready(index))
            me_should_wait_ready[index] = 1;

        if(!S_Can_Start(index))
            return;

        Me_Init(index);
        TIMER_STOP(me_stamp[index]);
        me_state[index] = ME_STATE_WORKING;
        Set_Starts_Led(index, LED_ON_CONST);
        ro_word.server_status = STATUS_IDLE;
        break;
    case ME_STATE_WORKING:
        if(ps_server_busy)
            return;
        event = Take_Event(index);
        if(event == EVENT_BUF_EMPTY || event == EVENT_LIMIT_ERROR || event == EVENT_EXPORT_READY_ERROR
                || ro_word.server_status != STATUS_IDLE)
        {

            me_lastEvent[index] = event;
            if(++me_retry[index] >= me_max_retry_times[index])
            {
                high_priority_me = index ? KEY_STARTL:KEY_STARTR;
                me_state[index] = ME_STATE_RESET;
                Do_Action_Reset_NoWait(index);
                if(event == EVENT_BUF_EMPTY)
                    Set_Starts_Led(index, LED_OFF_CONST);
                else
                {
                    Set_Starts_Led(index, LED_BLINK_ERR);
                    Start_Beep(BEEP_ERR);
                }
                break;
            }
            else
            {
                // 如果空事件,未到达最大次数重新等待
                if(event == EVENT_BUF_EMPTY)
                {
                    me_state[index] = ME_STATE_WAIT;
                    me_should_wait_ready[index] = 0;
                    Set_Starts_Led(index, LED_BLINK_INFO);
                    high_priority_me = index ? KEY_STARTL : KEY_STARTR;
                }
                if(ro_word.server_status != STATUS_IDLE)
                    ro_word.server_status = STATUS_IDLE;
            }
        }
        else
        {

        }


        // 重试动作次数小于最大重试次数，忽略错误
        ignore_error = (me_retry[index] < MAX_RETRY_TIMES);
        if(Can_Lock_Export(ignore_error) && index < KEY_STOP && S_Can_Start(index))
        {
            // 首次推送，如果不需要等待不等待
            bit me_not_need_wait = !auto_key_pending[index] && !me_should_wait_ready[index];

            if(me_stamp[index] == TIMER_STOPED )
                TIMER_START(me_stamp[index]);
            if((me_stamp[index] != TIMER_STOPED && TIMER_OUT(me_stamp[index],1000000)) || me_not_need_wait)
            {
                me_should_wait_ready[index] = 0;
                TIMER_STOP(me_stamp[index]);
                ++auto_key_pending[index];
                if(!auto_key_pending[index])
                    ++auto_key_pending[index];

                rw_word.action = TRANS_IN;
                pending_action = 1;
                rw_word.action_arg[0] &= ~0x01;
                rw_word.action_arg[0] |= (index & 0x01);
                pending_role = !index ? ACTION_PENDING_KEY1_EVENT : ACTION_PENDING_KEY2_EVENT;
            }
        }
        break;
    case ME_STATE_RESET:
        if(Maybe_Terminated(index))
        {
            Set_Starts_Led(index, LED_BLINK_WARNINNG);
            me_state[index] = ME_STATE_IDLE;
            break;
        }

        // 等待电机复位完成
        if(srd_desc[index].srd->running)
            return;

        import_need_close_gate[index] = 1;

        Set_Starts_Led(index, LED_OFF_CONST);

        if(!srd_desc[S1 + index].srd->hit_home)
        {
            Set_RO_Bit(S1_HOME_SENSOR_ERROR + index);
            ro_word.server_status = S1_STATUS_HOME_SENSOR_ERROR + index;
            Set_Starts_Led(index, LED_BLINK_ERR);
        }
        else
        {
            Reset_RO_Bit(S1_HOME_SENSOR_ERROR + index);
        }

        me_state[index] = ME_STATE_IDLE;

        break;
    }

    if(!ps_server_busy && !pending_action)
    {
        if(import_need_close_gate[index])
        {
            pending_action = 1;
            rw_word.action = CLOSE_BUFL_GATE + index;
            import_need_close_gate[index] = 0;
        }
    }
}

bit Fix_Level()
{
    static uchar fix_filter = 0xff;
    static uchar fix_sum = 8;
    bit state = IO_Read(IO_RACK_FIXED);
    fix_sum -= ((fix_filter & 0x80) >> 7);
    fix_filter = (fix_filter << 1) | state;
    fix_sum += state;
    return (fix_sum > 4);
}

void Reset_Fix()
{
    fix.index = 0;
    fix.exist = 0;
    fix.hat_exist = 0;
    fix.state = Fix_Level();
    fix.enable = 1;
}

void Update_fix()
{
    bit fix_level;

    if(!fix.enable)
        return;

    fix_level = Fix_Level();
    if(fix.state != fix_level)
    {
        fix.state = fix_level;
        if(fix.state)
        {
            if(IO_Read(IO_HAT_EXIST))
                fix.hat_exist |= (1 << fix.index);
            else
                fix.hat_exist &= ~(1 << fix.index);
            fix.hat_exist &= 0x1F;

            if(IO_Read(IO_TUBE_EXIST))
                fix.exist |= (1 << fix.index);
            else
                fix.exist &= ~(1 << fix.index);
            fix.exist &= 0x1F;
            fix.exist |= fix.hat_exist ? (1 << 5) : 0;
            ++fix.index;
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
// machine base

void Step(uchar role, bit level)
{
    //IO_Z_STEP(level);
}

void Start_Timer(uchar role)
{
    timer_enable(srd_desc[role].timer);
}

void Set_Timer(uchar role)
{
    ushort micro = srd_desc[role].srd->micro;
    ushort tick = FREQ_TABLE(srd_desc[role].srd->speed_index);
    if(micro)
    {
        // 默认使用16细分，如果使用了其他细分,改变pwm的频率
        tick = tick * FACILITYMICRO / (micro > FACILITYMICRO ? FACILITYMICRO : micro);
        tick = tick > MAX_C ? MAX_C : tick;
    }
    timer_autoreload_value_config(srd_desc[role].timer, tick*2);
    timer_channel_output_pulse_value_config(srd_desc[role].timer, srd_desc[role].timer_ch, tick);
}

void Stop_Timer(uchar role)
{
    timer_disable(srd_desc[role].timer);
}

// 启动电机
void Start_Motor(uchar role)
{
    SpeedRampData *srd = srd_desc[S1+role].srd;
    long *distance = (long*)&ro_word.mi[S1+role].distance;
    *distance = 0;
    if(srd->dir)
        IO_Set(IO_S1_CW + role);
    else
        IO_Reset(IO_S1_CW + role);
    Set_Timer(srd->role);
    Start_Timer(srd->role);
    IO_Set(IO_S1_EN + role);
    IO_Reset(IO_S1_LOCK + role);
}

// 停止电机
void Stop_Motor(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    long  *distance = (long*)&ro_word.mi[role].distance;
    ushort *duration = (ushort*)&ro_word.mi[role].duration;
    if (srd->dir == CCW)
        *distance = -srd->step_count;
    else
        *distance = srd->step_count;
    *duration = (ushort)((SystemTick() - srd->start_time)/US_TO_TICK(1000));
    srd->need_cleanup = 0;
    srd->running = 0;
    IO_Set(IO_S1_LOCK + role);
    IO_Reset(IO_S1_EN + role);  // debug
}

void Init_Move(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;
    uchar limit = srd_desc[role].limit;
    uchar limit2 = srd_desc[role].limit2;
    uchar limit3 = srd_desc[role].limit3;

    srd->init = Start_Motor;
    srd->step = Step;
    srd->start_timer = Set_Timer;
    srd->stop_timer = Stop_Timer;
    srd->cleanup = Stop_Motor;
    srd->home_state = IO_Read(home);
    srd->limit_state = IO_Read(limit);
    srd->limit_state2 = IO_Read(limit2);
    srd->limit_state3 = IO_Read(limit3);
    srd->hit_home = 0;
    srd->hit_limit = 0;
    srd->hit_limit2 = 0;
    srd->hit_limit3 = 0;
}

// 执行移动指令时, 每一步调用一次
void Move_To_Update(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;
    uchar limit = srd_desc[role].limit;
    uchar limit2 = srd_desc[role].limit2;
    uchar limit3 = srd_desc[role].limit3;

    Update_IO();

    if(IO_Read(home) != srd->home_state)
    {
        srd->home_state = !srd->home_state;
        srd->hit_home = 1;
        ro_word.pos[role] = 0;
        srd->stamp = SystemTick();
        srd->step_left = labs(srd->target);
    }
    else if(limit && (IO_Read(limit) != srd->limit_state))
    {
         srd->limit_state = !srd->limit_state;
         srd->hit_limit  = 1;
         if(srd->limit_state && srd->step_left > srd->limit_step)
            srd->step_left = srd->limit_step;
    }
    else if(limit2 && (IO_Read(limit2) != srd->limit_state2))
    {
         srd->limit_state2 = !srd->limit_state2;
         srd->hit_limit2 = 1;
         if(srd->limit_state2 && srd->step_left > srd->limit2_step)
            srd->step_left = srd->limit2_step;
    }
    else if(limit3 && (IO_Read(limit3) != srd->limit_state3))
    {
         srd->limit_state3 = !srd->limit_state3;
         srd->hit_limit3  = 1;
         if(srd->limit_state3 && srd->step_left > srd->limit3_step)
            srd->step_left = srd->limit3_step;
    }
    else
        ro_word.pos[role] += srd->step_offset;
}

void T1_Move_To_Update(uchar role)
{
    Move_To_Update(role);
    Update_fix();
}

// 执行移动指令时, 每一步调用一次
void Reset_To_Update(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;
    uchar limit = srd_desc[role].limit;

    Update_IO();

    if(IO_Read(home) != srd->home_state)
    {
        srd->home_state = !srd->home_state;
        srd->hit_home = 1;
        ro_word.pos[role] = 0;
        srd->stamp = SystemTick();
        srd->step_left = labs(srd->target);
    }
    if(IO_Read(limit) != srd->limit_state)
    {
         srd->limit_state = !srd->limit_state;
         srd->hit_limit  = 1;
    }
    else
        ro_word.pos[role] += srd->step_offset;
}

void Do_Action_Stop(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;

    if (srd->running)
    {
        Stop_Timer(role);
        Stop_Motor(role);
    }
}

bit Do_Action_Reset(uchar role, ushort reset_speed)
{
    SpeedRampData *srd = srd_desc[role].srd;

    if(Maybe_Terminated(role))
        return 0;

    Init_Move(role);
    srd->update = Reset_To_Update;
    srd->target = srd->home_state ? srd->reset_step : -srd->reset_step;
    speed_cntr_Move(srd,
                    srd->home_state ? rw_word.mp[role].max_distance : -rw_word.mp[role].max_distance,
                    reset_speed);

    while(srd->running)
    {
        if(Maybe_Terminated(role))
            return 0;
        Process_Main_Loop();
    }

    if(srd_desc[role].home && !srd->hit_home)
    {
        Set_RO_Bit(S1_HOME_SENSOR_ERROR + role );
        ro_word.server_status = S1_STATUS_HOME_SENSOR_ERROR + role;
        return 0;
    }
    else
        Reset_RO_Bit(S1_HOME_SENSOR_ERROR + role);

    // 复位完成，移动到负坐标位置
    Do_Action_Move_To(role, -srd->reset_step, rw_word.mp[role].max_speed);
    return 1;
}

void Do_Action_Reset_NoWait(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;

    ushort speed = rw_word.mp[role].max_speed;
    if(role == S1 || role == S2)
        speed = ((ushort*)&rw_word.s1_max_speed2)[role];

    Init_Move(role);
    srd->update = Reset_To_Update;
    srd->target = srd->home_state ? srd->reset_step : -srd->reset_step;
    speed_cntr_Move(srd,
                    srd->home_state ? rw_word.mp[role].max_distance : -rw_word.mp[role].max_distance,
                    speed);
}

void Do_Action_Move_To(uchar role, long target, ushort speed)
{
    SpeedRampData *srd = srd_desc[role].srd;

    Init_Move(role);
    srd->update = Move_To_Update;
    srd->target = target;

    speed_cntr_Move(srd,
                    target - ro_word.pos[role],
                    speed);

    while(srd->running)
        Process_Main_Loop();
}

// machine base end
///////////////////////////////////////////////////////////////

void Clear_Limit(uchar role)
{
    srd_desc[role].limit2 = NULL_IN;
    srd_desc[role].limit3 = NULL_IN;
}

void Init_S_Move(uchar role)
{
    Init_Move(role);
}

//停机，停止所有在运行的电机
void Do_Action_S_Stop(uchar role)
{
    Do_Action_Stop(role);
}

void Do_Action_S_Move_To(uchar role, long target, ushort speed)
{
    Do_Action_Move_To(role, target, speed);
}

// V1/V2/V3/V4 打开
// role : V1;V2;V3;V4
uchar Do_Action_V_Open(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;

    if(IO_Read(home))
    {
        // 在复位位置，且坐标不正确，清除坐标，
        // 因为检测到复位光耦遮挡，且坐标为关闭闸门坐标值，发送打开/关闭阀门将会不动作
        if(ro_word.pos[role] > srd->reset_step*2)
            ro_word.pos[role] = 0;
        return 1;
    }

    return Do_Action_Reset(role,rw_word.mp[role].max_speed);
}

// V1/V2/V3/V4 关闭
// role : V1;V2;V3;V4
bit Do_Action_V_Close(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    uchar home = srd_desc[role].home;

    assert(role >= V1 && role <= V3);

    Init_Move(role);
    srd->update = Move_To_Update;
    srd->target = ((short*)&rw_word.v1_close_pos)[role - V1];

    speed_cntr_Move(srd,
                    srd->target- ro_word.pos[role],
                    rw_word.mp[role].max_speed);

    while(srd->running)
        Process_Main_Loop();

    // 关闭闸门动作后还在复位(打开闸门)位置
    if(IO_Read(home))
    {
        ro_word.server_status = V1_STATUS_MOTOR_ERROR + role - V1;
        Set_RO_Bit(V1_MOTOR_ERROR + role - V1);
        return 0;
    }
    else
        Reset_RO_Bit(V1_MOTOR_ERROR + role - V1);
    return 1;
}

/*!
 * V1/V2/V3/V4 打开闸门
 * role : V1;V2;V3;V4
 * \brief Do_Action_V_Open_Gate
 * V1 V2 带有传动结构，打开阀门时实际是自身执行了关闭动作
 */
uchar Do_Action_V_Open_Gate(uchar role)
{
    if(role == V1 || role == V2)
        return Do_Action_V_Close(role);
    else if(role == V3)
        return Do_Action_V_Open(role);
    return 1;
}

/*!
 * V1/V2/V3/V4 关闭闸门
 * role : V1;V2;V3;V4
 * \brief Do_Action_V_Open_Gate
 * V1 V2 带有传动结构，关闭阀门时实际是自身执行了打开动作
 */
bit Do_Action_V_Close_Gate(uchar role)
{
    if(role == V1 || role == V2)
        return Do_Action_V_Open(role);
    else if(role == V3)
        return Do_Action_V_Close(role);
    return 1;
}

//////////////////////////////////////////////////////////////////////////////////

void Do_Action_StandBy()
{
    uchar i = 0;
    for(i = 0; i < MACHINE_COUNT; ++i)
        Do_Action_Stop(i);
}

void Do_Action_Home()
{
    uchar i = 0;
    uchar s_reset_flag[2] = {0};
    if(!IO_Read(IO_PREL_EXIST))
    {
        Do_Action_Reset_NoWait(S1);
        s_reset_flag[0] = 1;
    }
    if(!IO_Read(IO_PRER_EXIST))
    {
        Do_Action_Reset_NoWait(S2);
        s_reset_flag[1] = 1;
    }
    Do_Action_Reset_NoWait(V1);
    Do_Action_Reset_NoWait(V2);
    Do_Action_Reset_NoWait(V3);
    while(srd_desc[V1].srd->running || srd_desc[V2].srd->running || \
          srd_desc[V3].srd->running )
    {
        Maybe_Terminated(S1);
        Maybe_Terminated(S2);
        Process_Main_Loop();
    }
    for(i = 0; i < 3; ++i)
    {
        if(!srd_desc[V1 + i].srd->hit_home)
        {
            Write_RO_bit(V1_HOME_SENSOR_ERROR + i, 1, 1);
            ro_word.server_status = V1_STATUS_HOME_SENSOR_ERROR + i;
        }
        else
            Write_RO_bit(V1_HOME_SENSOR_ERROR + i, 0, 1);
    }

    // 复位完成后关闭闸门
    Do_Action_V_Close_Gate(V1);
    Do_Action_V_Close_Gate(V2);
    Do_Action_V_Close_Gate(V3);
    // 等待推入推出电机复位完成
    while(srd_desc[S1].srd->running || srd_desc[S2].srd->running)
    {
        Maybe_Terminated(S1);
        Maybe_Terminated(S2);
        Process_Main_Loop();
    }

    Do_Action_Reset_NoWait(V1);
    Do_Action_Reset_NoWait(V2);
    Do_Action_Reset_NoWait(V3);
    while(srd_desc[V1].srd->running || srd_desc[V2].srd->running || \
          srd_desc[V3].srd->running )
    {
        Process_Main_Loop();
    }
    for(i = 0; i < 3; ++i)
    {
        ushort valid_distance = ((short*)&rw_word.v1_close_pos)[i];
        valid_distance = (ushort)((float)valid_distance*0.75f);
        if(abs(ro_word.mi[V1+i].distance) < valid_distance)
            Do_Action_V_Open_Gate(V1 + i);
        else
            Do_Action_V_Close_Gate(V1 + i);
    }

    for(i = 0; i < 2; ++i)
    {
        if(!s_reset_flag[i])
            continue;

        if(!srd_desc[S1 + i].srd->hit_home)
        {
            Set_RO_Bit(S1_HOME_SENSOR_ERROR + i);
            ro_word.server_status = S1_STATUS_HOME_SENSOR_ERROR + i;
        }
        else
        {
            Reset_RO_Bit(S1_HOME_SENSOR_ERROR + i);
            // 复位完成，移动到负坐标位置
            Do_Action_Move_To(S1 + i, -srd_desc[S1 + i].srd->reset_step, rw_word.mp[S1 + i].max_speed);
        }
    }

    Start_Beep(ro_word.server_status == STATUS_IDLE ? BEEP_INFO : BEEP_ERR);
}

void Do_Action_Selftest(void)
{
    Update_IO();
    if( Rack_Exist(IMPORTL) || Rack_Exist(EXPORT) || Rack_Exist(IMPORTR))
    {
        ro_word.server_status = EXEC_STATUS_CONDITIONS_ERROR;
        ro_bit.exec_conditions_error = 1;
        return;
    }
    else
    {
        ro_bit.exec_conditions_error = 0;
    }

    if(!Do_Action_V_Open_Gate(V1))
        return;
    if(!Do_Action_V_Open_Gate(V2))
        return;
    if(!Do_Action_V_Open_Gate(V3))
        return;
    if(ro_word.server_status == STATUS_IDLE)
    {
        srd_desc[T1].limit = IO_EXPORT_READY;
        srd_desc[T1].srd->limit_step = T1_IMPORT_LIMIT_BUFFER_STEP;
        Do_Action_T_Trans_To_Target(T1, rw_word.mp[T1].max_distance, rw_word.mp[T1].max_speed);
    }

    Update_IO();
    if( Rack_Exist(IMPORTL) || Rack_Exist(EXPORT) || Rack_Exist(IMPORTR))
    {
        ro_word.server_status = EXEC_STATUS_CONDITIONS_ERROR;
        ro_bit.exec_conditions_error = 1;
        return;
    }
    else
    {
        ro_bit.exec_conditions_error = 0;
    }

    Do_Action_Home();
}

void Do_Action_Disable()
{
    uchar i = 0;
    for(i = 0; i < MACHINE_COUNT; ++i)
        IO_Reset(IO_S1_EN + i);
}


bit Do_Action_S_Push_Racks_To_Import(bit home_s, uchar role)
{
    bit result = 1;
    SpeedRampData *srd = srd_desc[role].srd;
    uchar s_index = role - S1;
    uchar another_role = (role == S1 ? S2 : S1);

    Clear_Limit(role);

    srd_desc[role].limit = (role == S1 ? IO_IMPORTL_READY : IO_IMPORTR_READY);
    srd_desc[role].limit2 = (role == S1 ? IO_S1_LIMIT : IO_S2_LIMIT);

    // 如果试管架已在目标节点
    if(Rack_Exist(IMPORTL) || Rack_Exist(IMPORTR) || Rack_Exist(EXPORT))
    {
        return 1;
    }

    if(!Do_Action_V_Close_Gate(s_index ? V1 : V2))
        return 0;
    if(!Do_Action_V_Open_Gate(s_index ? V2 : V1))
        return 0;

    // 如果另一个电机在限位位置，执行一次复位,以防试管架传输时碰到推手
    if(IO_Read(role == S1 ? IO_S2_LIMIT : IO_S1_LIMIT))
    {
        if(!IO_Read(srd_desc[another_role].home))
            Do_Action_Reset(another_role,((ushort*)&rw_word.s1_max_speed2)[another_role]);
    }

    // 如果在电机限位位置，执行一次复位动作
    // 同时电机在最大位置pos为max_distance 时,可能为电机丢步并未运行,可执行一次复位动作
    // 只要不是在(不含首次)自动推送模式,不在复位位置应当复位一次
    if(IO_Read(srd_desc[role].limit2) || ro_word.pos[role] >= rw_word.mp[role].max_distance
            || (!IO_Read(srd_desc[role].home) && auto_key_pending[s_index] < 2))
    {
        if(!IO_Read(srd_desc[role].home))
            Do_Action_Reset(role,((ushort*)&rw_word.s1_max_speed2)[s_index]);
    }

    Init_Move(role);
    srd->update = Move_To_Update;
    srd->target = rw_word.mp[role].max_distance;

    speed_cntr_Move(srd,
                    srd->target - ro_word.pos[role],
                    rw_word.mp[role].max_speed);

    mkey_changed[role] = 0;
    while(srd->running)
    {
        Process_Main_Loop();
    }

    if(!srd->hit_limit && !Ignore_Import_Ready(s_index))
    {
        if(!srd->hit_limit2)
        {
            Post_Event(role, EVENT_LIMIT_ERROR);
            if(!mkey_changed[role])
            {
                Set_RO_Bit(S1_LIMIT_SENSOR_ERROR + s_index);
                ro_word.server_status = S1_STATUS_LIMIT_SENSOR_ERROR + s_index;
            }
            return 0;
        }
        else
        {
            Reset_RO_Bit(S1_LIMIT_SENSOR_ERROR + s_index);
        }

        if(!Ignore_Import_Ready(s_index))
        {
            /*！ 这里不需要置错误状态，因为这只表示试管架缓存空了 */
            result = 0;
        }
        else
        {
            Reset_RO_Bit(RACK_READY_IMPORTL_ERROR + s_index);
            ro_word.rack_importl_refer_count += 1;
        }
    }
    else
    {
        Reset_RO_Bit(S1_LIMIT_SENSOR_ERROR + s_index);
        Reset_RO_Bit(RACK_READY_IMPORTL_ERROR + s_index);
        ro_word.rack_importl_refer_count += 1;
    }

    if(home_s)
    {
         Do_Action_Reset(role, rw_word.mp[role].max_speed);
    }

    if(ro_word.server_status == STATUS_IDLE && me_state[role - S1] == ME_STATE_IDLE)
        Do_Action_V_Close_Gate(V1 + s_index);

    if(srd->hit_limit2)
    {
        Post_Event(role, EVENT_BUF_EMPTY);
        Process_Main_Loop();
        // 如果为空,关闭闸门
        // Do_Action_V_Close_Gate(V1 + role);
    }

    return result;
}

void Do_Action_T1_Trans_To_Export_Ready(uchar from_s)
{
    uchar role = T1;
    bit active_scan_only = 0;
    SpeedRampData *srd = srd_desc[role].srd;
    Clear_Limit(role);
    srd_desc[role].limit = IO_EXPORT_READY;

    // T1输出区节点应无试管架
    if(rack_ready_export && !Ignore_Export_Ready())
        return;
    else
        Reset_RO_Bit(EXEC_CONDITIONS_ERROR);

    if(!Do_Action_V_Close_Gate(V3))
        return;

    // 按钮请求动作下，检测到上一次为到位错误事件，且扫描到了部分的条码
    if(me_state[from_s] == ME_STATE_WORKING && me_lastEvent[from_s] == EVENT_EXPORT_READY_ERROR
            && Mbar_Count() && !Rack_Scan_Finished())
        active_scan_only = 1;

    rack_ready_export = 0;
    if(!rw_word.app_arg1.disable_scanner)
    {
        if(!active_scan_only)
            Start_Scan(0);
        else
            Active_Scan();
    }

    if(!active_scan_only)
        Reset_Fix();

    Init_Move(role);
    ro_word.pos[role] = 0;
    srd->update = T1_Move_To_Update;
    srd->target = srd->limit_target;
    srd->limit_step = T1_EXPORT_LIMIT_BUFFER_STEP;
    if(IO_Read(srd_desc[role].limit) && !Ignore_Export_Ready())
    {
        // 试管架存在,向前走少量距离，确保到位
        srd->target = srd->limit_step;
        srd->hit_limit = 1;
        fix.enable = 0;
    }

    speed_cntr_Move(srd,
                    srd->target - ro_word.pos[role],
                    rw_word.mp[role].max_speed);

    while(srd->running)
    {
        Beep_Cntrol();
        Process_Main_Loop();
    }

    if(!srd->hit_limit && !Ignore_Export_Ready())
    {
        // 运行过程中读取到了条码,且还没有到位
        if(!rw_word.app_arg1.disable_scanner && Mbar_Count())
        {
            if(!Rack_Scan_Finished())
            {
                Set_RO_Bit(RACK_READY_EXPORT_ERROR);
                ro_word.server_status = RACK_STATUS_READY_EXPORT_ERROR;

                Post_Event(from_s&0x01, EVENT_EXPORT_READY_ERROR);
            }
            else
            {
                Reset_RO_Bit(RACK_READY_EXPORT_ERROR);
                ro_word.rack_flag = fix.exist;
                ++ro_word.rack_number;
                rack_ready_export = 1;
            }
            // 只要读取到条码，就认为试管架输入到此站点了
            ro_word.rack_importl_refer_count = 0;
            ro_word.rack_importr_refer_count = 0;
            ro_word.rack_export_refer_count += 1;
        }
        else {
             // 未检测到试管架信息，可能轨道内无试管架
            Post_Event(from_s&0x01, EVENT_BUF_EMPTY);
            ro_word.rack_importl_refer_count = 0;
            ro_word.rack_importr_refer_count = 0;
            ro_word.rack_export_refer_count = 0;
        }
    }
    else
    {
        Reset_RO_Bit(RACK_READY_EXPORT_ERROR);
        ro_word.rack_flag = fix.exist;
        ++ro_word.rack_number;
        ro_word.rack_importl_refer_count = 0;
        ro_word.rack_importr_refer_count = 0;
        ro_word.rack_export_refer_count += 1;
        rack_ready_export = 1;
        // 当hit_limit == 1,此时还在运行过程中，手动取走试管架
        // 则可能会导致Update_Io 时，rack_ready_export 不再会被清除
    }
    Stop_Scan(0);
}

// 辅助传感器检查到试管架存在 返回1
bit Auxiliary_Rack_Exist()
{
    if(fixed_active && !IO_Read(IO_RACK_FIXED))
        return 1;
    if(tube_active && IO_Read(IO_TUBE_EXIST))
        return 1;
    if(hat_active && IO_Read(IO_HAT_EXIST))
        return 1;
    return 0;
}

void Clear_Auxiliary()
{
    fixed_active = 0;
    tube_active = 0;
    hat_active = 0;
}

void Do_Action_Trans_In()
{
    bit role = rw_word.action_arg[0] & 0x01 ? S2 : S1;
    bit result = 0;

     // 检测到试管架存在传送区，先传送一次
    if(Rack_Exist(IMPORTL) || Rack_Exist(IMPORTR) || Rack_Exist(EXPORT) \
        || Auxiliary_Rack_Exist())
    {
        Clear_Auxiliary();
        if(!rack_ready_export)
            Do_Action_T1_Trans_To_Export_Ready(role);
    }

    result = Do_Action_S_Push_Racks_To_Import(0, role);

    // 用户通过按钮取消了传输
    if((me_state[role] != ME_STATE_WORKING) && (pending_role == (!role ? ACTION_PENDING_KEY1_EVENT : ACTION_PENDING_KEY2_EVENT)))
        return;

    if(result && ro_word.server_status == STATUS_IDLE)
    {
        // 没有错误，传输试管架
        Do_Action_T1_Trans_To_Export_Ready(role);
    }

    if(!result && ro_word.server_status == (S1_STATUS_LIMIT_SENSOR_ERROR + role))
    {
        // S限位错误，有可能试管架在export，相互挤压导致s推送不到位，尝试一次传输试管架
        Do_Action_T1_Trans_To_Export_Ready(role);
        // 没有检测到试管架，重置之前的错误
        if(ro_word.rack_export_refer_count == 0)
        {
            Post_Event(role, EVENT_LIMIT_ERROR);
            ro_word.server_status = (S1_STATUS_LIMIT_SENSOR_ERROR + role);
        }
        else
            ro_word.server_status = STATUS_IDLE;

    }

}

void Do_Action_T1_Trans_Back_Import(bit outfrom)
{
    uchar role = T1;
    SpeedRampData *srd = srd_desc[role].srd;
    Clear_Limit(role);

    srd_desc[role].limit = IO_IMPORTR_READY;

    if(Rack_Exist(IMPORTL) || Rack_Exist(IMPORTR))
    {
        // 试管架存在
        Set_RO_Bit(EXEC_CONDITIONS_ERROR);
        ro_word.server_status = EXEC_STATUS_CONDITIONS_ERROR;
        return;
    }
    else
        Reset_RO_Bit(EXEC_CONDITIONS_ERROR);

    if(outfrom)
    {
        if(!Do_Action_V_Open_Gate(V3))
            return;
    }

    Init_Move(role);
    ro_word.pos[role] = 0;
    srd->update = Move_To_Update;
    srd->target = -srd->limit_target;
    srd->limit_step = T1_BACK_LIMIT_BUFFER_STEP;


    speed_cntr_Move(srd,
                    srd->target - ro_word.pos[role],
                    rw_word.mp[role].max_speed);

    while(srd->running)
        Process_Main_Loop();

    if(!srd->hit_limit)
    {
        Set_RO_Bit(RACK_READY_EXPORT_ERROR);
        ro_word.server_status = RACK_STATUS_READY_EXPORT_ERROR;
    }
    else
    {
        Reset_RO_Bit(RACK_READY_EXPORT_ERROR);
    }
}

void Do_Action_T1_Trans_Out()
{
    uchar role = T1;
    bit reverse = 0;
    SpeedRampData *srd = srd_desc[role].srd;

    Clear_Limit(role);
    srd_desc[role].limit = IO_EXPORT_READY;

    if(!Do_Action_V_Open_Gate(V3))
        return;

    rw_bit.export_stop_triggered = 0;
    Init_Move(role);
    ro_word.pos[role] = 0;
    srd->update = T1_Move_To_Update;
    srd->target = reverse ? -rw_word.mp[role].max_distance : rw_word.mp[role].max_distance;
    srd->limit_step = reverse ? (T1_OUT_BUFFER_STEP << 1) : T1_OUT_BUFFER_STEP;

    speed_cntr_Move(srd,
                    srd->target- ro_word.pos[role],
                    rw_word.mp[role].max_speed);

    while(srd->running)
    {
        Process_Main_Loop();
        if(rw_bit.export_stop_triggered)
        {
            rw_bit.export_stop_triggered = 0;
            srd->step_left = T1_EXPORT_LIMIT_BUFFER_STEP;
        }
    }

    if(!srd->hit_limit && !Ignore_Export_Ready())
    {
        Set_RO_Bit(RACK_READY_EXPORT_ERROR);
        ro_word.server_status = RACK_STATUS_READY_EXPORT_ERROR;
    }
    else
    {
        Reset_RO_Bit(RACK_READY_EXPORT_ERROR);
        ro_word.rack_importl_refer_count = 0;
        ro_word.rack_importr_refer_count = 0;
        ro_word.rack_export_refer_count  = 0;
    }

    rack_ready_export = 0;

    // 检测到不遮挡或忽略传感器才关闭闸刀
    if(!IO_Read(IO_EXPORT_READY) || Ignore_Export_Ready())
        Do_Action_V_Close_Gate(V3);

}

void Do_Action_S_T1_Trans_In_Out()
{
    bit role = rw_word.action_arg[0] & 0x01 ? S2 : S1;
    if(Do_Action_S_Push_Racks_To_Import(0, role))
        Do_Action_T1_Trans_Out();
}

void Do_Action_Trans_Next_Station()
{
    Do_Action_Trans_In();
}

void Do_Action_T_Trans_To_Target(uchar role, long target, ushort speed)
{
    assert(role == T1);
    ro_word.pos[role] = 0;
    Do_Action_Move_To(role, target, speed);
}


////////////////////////////////////////////////////////////////////////////////
/// \brief Process_Action
///
void Process_Action()
{
    long target;
    uchar role;
    switch (rw_word.action & 0x00FF)
    {

    case STANDBY:
        Do_Action_StandBy();
        ps_server_busy = 0;
        break;
    case MRESET:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Home();
        ps_server_busy = 0;
        break;
     case SELFTEST:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Selftest();
        ps_server_busy = 0;
        break;
     case TRANS_IN:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Trans_In();
        ps_server_busy = 0;
        break;
    case T1_TRANS_OUT:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_T1_Trans_Out();
        ps_server_busy = 0;
        break;

    case S_T1_TRANS_IN_OUT:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_S_T1_Trans_In_Out();
        ps_server_busy = 0;
        break;
    case TRANS_NEXT_STATION:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Trans_Next_Station();
        ps_server_busy = 0;
        break;
    case S_PUSH_RACKS_TO_IMPORT:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        role = rw_word.action_arg[0] & 0x01 ? S2 : S1;
        Do_Action_S_Push_Racks_To_Import(0, role);
        ps_server_busy = 0;
        break;
    case T1_TRANS_TO_TARGET:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        target = (long)(rw_word.action_arg[0] << 16 | rw_word.action_arg[1]);
        role = T1 + rw_word.action - T1_TRANS_TO_TARGET;
        srd_desc[role].limit = NULL_IN;
        srd_desc[role].limit2 = NULL_IN;
        srd_desc[role].limit3 = NULL_IN;
        Do_Action_T_Trans_To_Target(role, target, rw_word.action_arg[2]);
        ps_server_busy = 0;
        break;
    case OPEN_BUFL_GATE:
    case OPEN_BUFR_GATE:
    case OPEN_EXPORT_GATE:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_V_Open_Gate(V1 + ((rw_word.action - OPEN_BUFL_GATE)>>1));
        ps_server_busy = 0;
        break;
    case CLOSE_BUFL_GATE:
    case CLOSE_BUFR_GATE:
    case CLOSE_EXPORT_GATE:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_V_Close_Gate(V1 + ((rw_word.action - CLOSE_BUFL_GATE)>>1));
        ps_server_busy = 0;
        break;
    case HOME_S1:
    case HOME_S2:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        if(!IO_Read(IO_S1_HOME + (rw_word.action - HOME_S1)))
        {
            Do_Action_Reset(S1 + (rw_word.action - HOME_S1),((ushort*)&rw_word.s1_max_speed2)[(rw_word.action - HOME_S1)]);
        }
        ps_server_busy = 0;
        break;
    case HOME_V1:
    case HOME_V2:
    case HOME_V3:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        if(!IO_Read(IO_V1_HOME + (rw_word.action - HOME_V1)))
        {
            Do_Action_Reset(V1 + (rw_word.action - HOME_V1),rw_word.mp[role].max_speed);
        }
        ps_server_busy = 0;
        break;
    case DISABLE_MOTOR:                             //解锁所有电机
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Disable();
        ps_server_busy = 0;
        break;

#ifdef PRODUCT_DEBUG
    case Z_MOVE_TO:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        Do_Action_Z_Move_To(rw_word.z_target,rw_word.z_max_speed);
        ps_server_busy = 0;
        break;
   case DISABLE_MOTOR:
        ps_server_busy = 1;
        ro_word.server_status = STATUS_IDLE;
        IO_Z_ENABLE(0);
        ps_server_busy = 0;
        break;
#endif
    default:
        break;
    }
}

void Main_Loop(void)
{
    uchar role;
    Update_WatchDog();
    LED_BLINK();

    Update_IO();

    for(role = S1; role < MACHINE_COUNT; ++role)
    {
        if (srd_desc[role].srd->need_cleanup && srd_desc[role].srd->cleanup)
            srd_desc[role].srd->cleanup(role);
    }

    // 一帧消息接收完成
    if (uart0_rx_pending && Uart0_FrameEnded())
    {
        uart0_rx_pending = 0;
        REN0 = 0;
        usart_interrupt_disable(USART2, USART_INT_RBNE);
        Process_Request();
    }
    // 应答结束, 允许接收
    if (!REN0 && !uart0_tx_pending)
    {
        uart0_len = 0;
        uart0_rx_pending = 0;
        REN0 = 1;
        usart_interrupt_enable(USART2, USART_INT_RBNE);
    }

    Process_Scanbar();
    Beep_Cntrol();
}


void Process_Main_Loop(void)
{
    Main_Loop();

    Process_Start_Event(KEY_STARTL);
    Process_Start_Event(KEY_STARTR);
    Process_Rack(KEY_STARTL);
    Process_Rack(KEY_STARTR);
    Key_Led_Ctrl(KEY_STARTL);
    Key_Led_Ctrl(KEY_STARTR);

    if (pending_action)
        longjmp(main_loop_env, 1);
}
